#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <cv.h>
#include <highgui.h>
#include <stdlib.h>
#include <math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <assert.h>
#include <iostream>
using namespace std;
using namespace cv;

#define JUMPPIXELS 1
#define RANGE 30
#define RANGE1 20
#define SEARCH_1CHANNEL_RANGE 1
#define HIST_RANGE 20
#define HIST_LOWER_BOUND 5
#define HIST_UPPER_BOUND 255

void plotHistogram1D(CvHistogram *hist, IplImage* histImg, int maxValue);

int search(uchar unique[][3],int count,uchar r, uchar g, uchar b)
{
  for(int i = 0;i<count;i++)
  {
    if(unique[i][0]==r && unique[i][1]==g && unique[i][2]==b)
      return i;
  }
  return -1;
}

void getMode(uchar borderR[],uchar borderG[],uchar borderB[],int len,uchar bgcolor[],int channels)
{
  uchar unique[len][3];
  int uniqueCount[len];
  int count=0;
  int maxfreq=1;
  int maxfreqind=0;
  for(int i=0;i<len;i++)
  {
    int pos;
    if((pos = search(unique,count,borderR[i],borderG[i],borderB[i]))>=0)
    {
      uniqueCount[pos]++;
      if(uniqueCount[pos]>maxfreq)
      {
	maxfreq = uniqueCount[pos];
	maxfreqind = i;
      }
    }
    else
    {
      unique[count][0]=borderR[i];
      unique[count][1]=borderG[i];
      unique[count][2]=borderB[i];
      uniqueCount[count++] = 1;
    }
  }
  bgcolor[0]=borderR[maxfreqind];
  bgcolor[1]=borderG[maxfreqind];
  bgcolor[2]=borderB[maxfreqind];
}

void getBackgroundColor(IplImage *img, uchar bgcolor[])
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar *data      = (uchar *)img->imageData;
  uchar borderR[2*(height+width-1)];
  uchar borderG[2*(height+width-1)];
  uchar borderB[2*(height+width-1)];
  int count=0;
  // Top Border
  for(int i=0;i<width;i++)
  {
    borderR[count] = data[i*channels];
    borderG[count] = data[i*channels+1];
    borderB[count] = data[i*channels+2];
    count++;
  }
  // Bottom Border
  for(int i=0;i<width;i++)
  {
    borderR[count] = data[(height-1)*step+i*channels];
    borderG[count] = data[(height-1)*step+i*channels+1];
    borderB[count] = data[(height-1)*step+i*channels+2];
    count++;
  }
  // Left Border
  for(int i=0;i<height;i++)
  {
    borderR[count] = data[i*step];
    borderG[count] = data[i*step+1];
    borderB[count] = data[i*step+2];
    count++;
  }
  // Right Border
  for(int i=0;i<height;i++)
  {
    borderR[count] = data[i*step+(width-1)*channels];
    borderG[count] = data[i*step+(width-1)*channels+1];
    borderB[count] = data[i*step+(width-1)*channels+2];
    count++;
  }
  getMode(borderR,borderG,borderB,count,bgcolor,channels);
}

bool inRange(uchar val1,uchar val2, int range)
{
  int diff = abs(val1-val2);
  return (diff<range);
}

void subtractBackground(IplImage *img)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar *data      = (uchar *)img->imageData;
  uchar bgcolor[channels];
  getBackgroundColor(img,bgcolor);
  for(int i=0;i<height;i++)
    for(int j=0;j<width;j++)
      if(inRange(data[i*step+j*channels],bgcolor[0],RANGE) && inRange(data[i*step+j*channels+1],bgcolor[1],RANGE) && inRange(data[i*step+j*channels+2],bgcolor[2],RANGE))
      {
	data[i*step+j*channels]=0;
	data[i*step+j*channels+1]=0;
	data[i*step+j*channels+2]=0;
      }
}

void thinChars(IplImage* img, int n)
{
  if(n==0)
    return;
  // To grayscale
  IplImage* grayImg = cvCreateImage( cvSize(img->width, img->height), IPL_DEPTH_8U, 1 );
  if(img->nChannels!=1)
  {
    cvCvtColor(img, grayImg, CV_BGR2GRAY );
  }
  else
  {
    cvCopy(img,grayImg);
  }
  
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* contour = 0;

  cvThreshold(grayImg, grayImg, 1, 255, CV_THRESH_BINARY );
  cvFindContours(grayImg, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  int count=0;
  for( ; contour != 0; contour = contour->h_next )
  {
//       CvScalar color = CV_RGB( 255, 255, 255 );
      CvScalar color = CV_RGB( 0, 0, 0);
      /* replace CV_FILLED with 1 to see the outlines */
      cvDrawContours(img, contour, color, color, -1, 1, 8 );
      count++;
  }
  cout<<"Contours:"<<count<<endl;
  cvReleaseImage(&grayImg);
  thinChars(img,n-1);
}

int cmpCvScalar(const void* arg1, const void* arg2, void* data)
{
  return ((*(CvScalar*)arg1).val[0]==(*(CvScalar*)arg2).val[0] && (*(CvScalar*)arg1).val[1]==(*(CvScalar*)arg2).val[1] && (*(CvScalar*)arg1).val[2]==(*(CvScalar*)arg2).val[2])?0:1;
}

void printSeq(CvSeq* seq)
{
  int total = seq->total;
  for(int i=0;i<total;i++)
  {
    CvScalar* ele = (CvScalar*)CV_GET_SEQ_ELEM(CvScalar,seq,i);
    cout<<"("<<ele->val[0]<<","<<ele->val[1]<<","<<ele->val[2]<<")";
  }
  cout<<endl;
}

void getModeColor(CvSeq* contour, IplImage* img, CvScalar *color)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  int total = contour->total;
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* seq = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvScalar), storage);
  for(int i=0;i<total;i++)
  {
    CvPoint *point =  (CvPoint *)CV_GET_SEQ_ELEM(CvPoint,contour,i);
    int x = point->x;
    int y = point->y;
    uchar r = data[y*step+x*channels];
    uchar g = data[y*step+x*channels+1];
    uchar b = data[y*step+x*channels+2];
    CvScalar c = cvScalar(r,g,b);
    cvSeqPush(seq,&c);
  }
  printSeq(seq);
  int maxFreq = 1;
  int maxFreqInd = 0;
  int uniqueFreq[total];
  int count = 0;
  CvMemStorage* storage1 = cvCreateMemStorage(0);
  CvSeq* unique = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvScalar), storage1);
  for(int i=0;i<total;i++)
  {
    int ind;
    if(cvSeqSearch(unique,(CvScalar*)CV_GET_SEQ_ELEM(CvScalar,seq,i),cmpCvScalar,0,&ind)==NULL)
    {
      cvSeqPush(unique,(CvScalar*)CV_GET_SEQ_ELEM(CvScalar,seq,i));
      uniqueFreq[count++]=1;
    }
    else
    {
      uniqueFreq[ind]++;
      if(uniqueFreq[ind]>maxFreq)
      {
	maxFreqInd = ind;
	maxFreq = uniqueFreq[ind];
      }
    }
  }
  CvScalar* ele = (CvScalar*)CV_GET_SEQ_ELEM(CvScalar,unique,maxFreqInd);
  (*color).val[0] = (*ele).val[0];
  (*color).val[1] = (*ele).val[1];
  (*color).val[2] = (*ele).val[2];
  printSeq(unique);
  cout<<"maxFreq:"<<maxFreq<<endl;
}

void getMeanColor(CvSeq* contour, IplImage* img, CvScalar *color)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  int total = contour->total;
  int sumr=0,sumg=0,sumb=0;
  for(int i=0;i<total;i++)
  {
    CvPoint *point =  (CvPoint *)CV_GET_SEQ_ELEM(CvPoint,contour,i);
    int x = point->x;
    int y = point->y;
    uchar r = data[y*step+x*channels];
    uchar g = data[y*step+x*channels+1];
    uchar b = data[y*step+x*channels+2];
    sumr+=r;
    sumg+=g;
    sumb+=b;
  }

  (*color).val[0] = sumr/total;
  (*color).val[1] = sumg/total;
  (*color).val[2] = sumb/total;
}

void colorSelected(IplImage* img, IplImage* final, CvSeq* colors, CvScalar toSet)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  int total = colors->total;
  cvZero(final);
  for(int i=0;i<height;i++)
    for(int j=0;j<width;j++)
    {
      bool flag=false;
      for(int k=0;k<colors->total;k++)
	if(inRange(data[i*step+j*channels],uchar((*(CvScalar*)CV_GET_SEQ_ELEM(CvScalar,colors,k)).val[0]),RANGE1) && inRange(data[i*step+j*channels+1],uchar((*(CvScalar*)CV_GET_SEQ_ELEM(CvScalar,colors,k)).val[1]),RANGE1) &&
	inRange(data[i*step+j*channels+2],uchar((*(CvScalar*)CV_GET_SEQ_ELEM(CvScalar,colors,k)).val[2]),RANGE1))
	{
	  final->imageData[i*step+j*channels]=toSet.val[0];
	  final->imageData[i*step+j*channels+1]=toSet.val[1];
	  final->imageData[i*step+j*channels+2]=toSet.val[2];
	  flag=true;
	  break;
	}
      if(!flag)
      {
	final->imageData[i*step+j*channels]=0;
	final->imageData[i*step+j*channels+1]=0;
	final->imageData[i*step+j*channels+2]=0;
      }
    }
}

void refineImage(IplImage* img, IplImage* orig)
{
  IplImage* grayImg = cvCreateImage( cvSize(img->width, img->height), IPL_DEPTH_8U, 1 );
  cvCvtColor(img, grayImg, CV_BGR2GRAY );
  
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* contour = 0;

  cvThreshold(grayImg, grayImg, 1, 255, CV_THRESH_BINARY );
  cvFindContours(grayImg, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  CvMemStorage* storage1 = cvCreateMemStorage(0);
  CvSeq* colors = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvScalar), storage1);
  for( ; contour != 0; contour = contour->h_next )
  {
    CvScalar color;
//     getModeColor(contour,img,&color);
    getMeanColor(contour,img,&color);
    cout<<"Colour:("<<color.val[0]<<","<<color.val[1]<<","<<color.val[2]<<")"<<endl;
    cvSeqPush(colors,&color);
  }
  colorSelected(orig,img,colors,cvScalar(255,255,255));
  cvReleaseImage(&grayImg);
}

// bool canBeRemoved(IplImage *img, int x, int y, int size, int n=3)
// {
//   assert(n%2==1);
//   int height    = img->height;
//   int width     = img->width;
//   int step      = img->widthStep;
//   int channels  = img->nChannels;
//   uchar* data   = (uchar*)img->imageData;
//   return true; // TODO Correct
// }
// 
// void thinChar(IplImage* img, CvSeq* contour, CvScalar color)
// {
//   int height    = img->height;
//   int width     = img->width;
//   int step      = img->widthStep;
//   int channels  = img->nChannels;
//   uchar* data   = (uchar*)img->imageData;
//   int total = contour->total;
//   for(int i=0;i<total;i++)
//   {
//     CvPoint *point =  (CvPoint *)CV_GET_SEQ_ELEM(CvPoint,contour,i);
//     int x = point->x;
//     int y = point->y;
//     if(canBeRemoved(img,x,y))
//     {
//       data[y*step+x*channels=color.val[0];
//       data[y*step+x*channels+1]=color.val[1];
//       data[y*step+x*channels+2]=color.val[2];
//     }
//   }
// }

int getColorArray(IplImage* img, IplImage* mask, CvRect rect, uchar colorR[], uchar colorG[], uchar colorB[])
{
  int len=0;
  
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  
  int mheight    = mask->height;
  int mwidth     = mask->width;
  int mstep      = mask->widthStep;
  int mchannels  = mask->nChannels;
  uchar* mdata   = (uchar*)mask->imageData;
  
  for(int x=rect.x;x<rect.x+rect.width;x++)
  {
    for(int y=rect.y;y<rect.y+rect.height;y++)
    {
      if(mdata[y*mstep+x*mchannels]>0)
      {
	colorR[len] = data[y*step+x*channels];
	colorG[len] = data[y*step+x*channels+1];
	colorB[len] = data[y*step+x*channels+2];
	len++;
      }
    }
  }
  return len;
}

void paintSelected(IplImage* img, IplImage* dst, uchar color[], CvRect rect)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  
  int dheight    = dst->height;
  int dwidth     = dst->width;
  int dstep      = dst->widthStep;
  int dchannels  = dst->nChannels;
  
  for(int x=rect.x;x<rect.x+rect.width;x++)
  {
    for(int y=rect.y;y<rect.y+rect.height;y++)
    {
      if(inRange(data[y*step+x*channels],color[0],RANGE) && inRange(data[y*step+x*channels+1],color[1],RANGE) && inRange(data[y*step+x*channels+2],color[2],RANGE))
      {
	dst->imageData[y*dstep+x*dchannels] = 255;
      }
    }
  }
}

int getColorArray1Channel(IplImage* img, IplImage* mask, CvRect rect, uchar colorR[])
{
  int len=0;
  
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  
  int mheight    = mask->height;
  int mwidth     = mask->width;
  int mstep      = mask->widthStep;
  int mchannels  = mask->nChannels;
  uchar* mdata   = (uchar*)mask->imageData;
  
  for(int x=rect.x;x<rect.x+rect.width;x++)
  {
    for(int y=rect.y;y<rect.y+rect.height;y++)
    {
      if(mdata[y*mstep+x*mchannels]>0)
      {
	colorR[len] = data[y*step+x*channels];
	len++;
      }
    }
  }
  return len;
}

void paintSelected1Channel(IplImage* img, IplImage* dst, uchar color, CvRect rect)
{
  int height    = img->height;
  int width     = img->width;
  int step      = img->widthStep;
  int channels  = img->nChannels;
  uchar* data   = (uchar*)img->imageData;
  
  int dheight    = dst->height;
  int dwidth     = dst->width;
  int dstep      = dst->widthStep;
  int dchannels  = dst->nChannels;
  
  for(int x=rect.x;x<rect.x+rect.width;x++)
  {
    for(int y=rect.y;y<rect.y+rect.height;y++)
    {
      if(inRange(data[y*step+x*channels],color,HIST_RANGE) && data[y*step+x*channels]>=HIST_LOWER_BOUND)
      {
	dst->imageData[y*dstep+x*dchannels] = 255;
      }
    }
  }
}

int search1Channel(uchar unique[],int count,uchar r)
{
  for(int i = 0;i<count;i++)
  {
    if(inRange(unique[i],r,SEARCH_1CHANNEL_RANGE))
      return i;
  }
  return -1;
}

void getMode1Channel(uchar colors[],int len,uchar *bgcolor, int channels=1)
{
  uchar unique[len];
  int uniqueCount[len];
  int count=0;
  int maxfreq=1;
  int maxfreqind=0;
  for(int i=0;i<len;i++)
  {
    int pos;
    if((pos = search1Channel(unique,count,colors[i]))>=0)
    {
      uniqueCount[pos]++;
      if(uniqueCount[pos]>maxfreq)
      {
	maxfreq = uniqueCount[pos];
	maxfreqind = i;
      }
    }
    else
    {
      unique[count]=colors[i];
      uniqueCount[count++] = 1;
    }
  }
  bgcolor[0]=colors[maxfreqind];
}

int getMaxHistValue(CvHistogram *hist, int lowerBound, int upperBound)
{
  float maxVal = -1;
  int maxId=-1;
  for(int i=lowerBound;i<=upperBound;i++)
  {
     if(cvQueryHistValue_1D(hist,i)>maxVal)
     {
       maxVal = cvQueryHistValue_1D(hist,i);
       maxId = i;
     }
  }
  return maxId;
}

void getSkeletons1Channel(IplImage* img, IplImage* dst, int channels=1, int n=1)
{
  // img: 1 channel -- dst:1 channel
  if(n==0)
    return;
  cvZero(dst);
  IplImage* grayImg = cvCreateImage( cvSize(img->width, img->height), IPL_DEPTH_8U, 1 );
  if(img->nChannels!=1)
  {
    cvCvtColor(img, grayImg, CV_BGR2GRAY );
  }
  else
  {
    cvCopy(img,grayImg);
  }
  cvThreshold(grayImg, grayImg, 1, 255, CV_THRESH_BINARY );
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* contour = 0;

  cvFindContours(grayImg, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  int count=0;
  int histMaxId,histMinId;
  for( ; contour != 0; contour = contour->h_next )
  {
//       CvScalar color = CV_RGB( 255, 255, 255 );
      CvRect rect = cvBoundingRect(contour,0);
//       uchar colors[rect.width*rect.height];
//       int len = getColorArray1Channel(img,grayImg,rect,colors);
//       uchar modeColor;
//       getMode1Channel(colors,len,&modeColor,3);
//       paintSelected1Channel(img,dst,,rect);
      // TODO construct histogram: normalize it and take peak
      count++;
      
      // Plotting Histograms
      int numBins = 256;
      float range[] = {0, 255};
      float *ranges[] = { range };
      CvHistogram *hist1 = cvCreateHist(1, &numBins, CV_HIST_ARRAY, ranges, 1);
      cvClearHist(hist1);
      cvSetImageROI(img,rect);
      cvCalcHist(&img, hist1, 0, 0);
      
      histMaxId=getMaxHistValue(hist1,HIST_LOWER_BOUND,HIST_UPPER_BOUND);
      cvResetImageROI(img);
      paintSelected1Channel(img,dst,histMaxId,rect);
//       int maxValue = 100;
//       IplImage *histImg1 = cvCreateImage(cvSize(256, maxValue), 8 ,1);
//       plotHistogram1D(hist1,histImg1,maxValue);
//       cvNamedWindow("temp", CV_WINDOW_AUTOSIZE); 
//       cvMoveWindow("temp", 100, 100);
//       cvShowImage("temp",histImg1);
//       cvNamedWindow("temp1", CV_WINDOW_AUTOSIZE); 
//       cvMoveWindow("temp1", 100, 300);
//       cvShowImage("temp1",img);
//       cvWaitKey(0);
// 	  cvReleaseImage(&histImg1);
  }
  cout<<"Contours:"<<count<<endl;
  cvReleaseImage(&grayImg);
}

void getSkeletons(IplImage* img, IplImage* dst, int channels, int n=1)
{
  // img: RGB after background subtraction -- dst:1 channel
  if(n==0)
    return;
  cvZero(dst);
  IplImage* grayImg = cvCreateImage( cvSize(img->width, img->height), IPL_DEPTH_8U, 1 );
  if(img->nChannels!=1)
  {
    cvCvtColor(img, grayImg, CV_BGR2GRAY );
  }
  else
  {
    cvCopy(img,grayImg);
  }
  cvThreshold(grayImg, grayImg, 1, 255, CV_THRESH_BINARY );
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* contour = 0;

  cvFindContours(grayImg, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  int count=0;
  for( ; contour != 0; contour = contour->h_next )
  {
//       CvScalar color = CV_RGB( 255, 255, 255 );
      CvScalar color = CV_RGB( 0, 0, 0);
      CvRect rect = cvBoundingRect(contour,0);
      uchar colorR[rect.width*rect.height];
      uchar colorG[rect.width*rect.height];
      uchar colorB[rect.width*rect.height];
      int len = getColorArray(img,grayImg,rect,colorR,colorG,colorB);
      uchar modeColor[3];
      getMode(colorR,colorG,colorB,len,modeColor,3);
      paintSelected(img,dst,modeColor,rect);
      count++;
  }
  cout<<"Contours:"<<count<<endl;
  cvReleaseImage(&grayImg);
}

void getBinary(IplImage* src, IplImage *dst)
{
  IplImage* temp = cvCreateImage(cvGetSize(src),src->depth,src->nChannels);
  cvCopy(src,temp);
  subtractBackground(temp);
  cvCvtColor(temp,dst,CV_BGR2GRAY);
  cvThreshold(dst, dst, 1, 255, CV_THRESH_BINARY );
  cvReleaseImage(&temp);
}

void plotHistogram1D(CvHistogram *hist, IplImage* histImg, int maxValue)
{
//   CvHistogram *hist = cvCreateHist(1, sizes, type, ranges, uniform);
  float histMax = 0;
  cvGetMinMaxHistValue(hist, 0, &histMax, 0, 0);
  float scale = (float)maxValue/histMax;
  cvZero(histImg);
  for(int i=1;i<=255;i++)
  {
     cvLine( histImg, cvPoint(i-1,cvRound(cvQueryHistValue_1D(hist,i-1))), cvPoint(i,cvRound(cvQueryHistValue_1D(hist,i))), cvScalar(255), 2, 8, 0);
  }
}